#! /usr/bin/env python

# Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Author: Adam Robertson

#LINE OF CODE
#Look into wubble_actions for arm drivers (action server examples too)
#Why is reach at failing?

PKG = 'test_manipulation'
NAME = 'grasp_action'

import roslib; roslib.load_manifest(PKG)
import rospy

from actionlib import SimpleActionClient
from geometry_msgs.msg import *
from wubble_actions.msg import *
from test_manipulation.msg import *

def move_to(frame_id, position, orientation, vicinity=0.0):
    goal = ErraticBaseGoal()
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.header.frame_id = frame_id
    goal.target_pose.pose.position = position
    goal.target_pose.pose.orientation = orientation
    goal.vicinity_range = vicinity

    client.send_goal(goal)
    client.wait_for_result()



# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
def move_arm(shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
    goal = SmartArmGoal()
    goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]

    arm_client.send_goal(goal)
    arm_client.wait_for_goal_to_finish()

    result = arm_client.get_result()
    if result.success == False:
        print "Action failed"
    else:
        print "Result: [" + str(result.arm_position[0]) + ", " + str(result.arm_position[1]) + \
            str(result.arm_position[2]) + ", " + str(result.arm_position[3]) + "]"


def reach_at(frame_id, x, y, z):
    goal = SmartArmGoal()
    goal.target_point = PointStamped()
    goal.target_point.header.frame_id = frame_id
    goal.target_point.point.x = x
    goal.target_point.point.y = y + .1
    goal.target_point.point.z = z + .14

    arm_client.send_goal(goal)
    arm_client.wait_for_goal_to_finish()

    result = arm_client.get_result()
    if result.success == False:
        print "Action failed"
    else:
        print "Result: [" + str(result.arm_position[0]) + ", " + str(result.arm_position[1]) + \
            str(result.arm_position[2]) + ", " + str(result.arm_position[3]) + "]"


def move_gripper(left_finger, right_finger):
    goal = SmartArmGripperGoal()
    goal.target_joints = [left_finger, right_finger]

    gripper_client.send_goal(goal)
    gripper_client.wait_for_goal_to_finish()

    result = gripper_client.get_result()
    if result.success == False:
        print "Action failed"
    else:
        print "Result: [" + str(result.gripper_position[0]) + ", " + str(result.gripper_position[1]) + "]"

#this method is now outdated
def grasp_action(frame_id, x, y, z):
    #initial offsets (in meters)
    xOffset = 0 #-0.263256
    yOffset = 0 # .392411
    zOffset = 0 #0.061589

    print "1)Going to the Cobra Pose."	
    #go to cobra pose
    move_arm(0.0, 1.972222, -1.972222, 0.0)

    print "2)Opening the gripper."
    #open the gripper	
    move_gripper(0.2, -0.2)
    #tilt wrist down (actuallt I think the cobra pose does this)

    #move the arm into the proper position
    print "3)Moving the arm into position [" + str(x) + "," + str(y) + "," + str(z) + "]."
    reach_at(frame_id, y + yOffset, x + xOffset, z + zOffset)

    print "4)Closing the gripper."
    #Close gripper
    move_gripper(-0.075, 0.075)
    print "5)Going to the Cobra Pose."	
    #back to the cobra pose
    move_arm(0.0, 1.972222, -1.972222, 0.0)

def test_grasp (frame_id, x, y, z):
    goal = SmartArmGoal()
    goal.target_point = PointStamped()
    goal.target_point.header.frame_id = frame_id
    goal.target_point.point.x = x 
    goal.target_point.point.y = y 
    goal.target_point.point.z = z 

    grasp_client.send_goal(goal)
    grasp_client.wait_for_goal_to_finish()

    result = grasp_client.get_result()
    if result.success:
        print "Grasp Action succeeded"
    else:
        print "Grasp Action failed"

if __name__ == '__main__':
    try:
        #initialize the node        
        rospy.init_node(NAME, anonymous=True)

        #initialize the clients to interface with 
        grasp_client = SimpleActionClient("pick_up_action", SmartArmGraspAction)
        #grasp_client.wait_for_server()
	client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
        client.wait_for_server()

	    # 	/map to /arm_base_link - Translation: [0.139, 0.000, 0.214]
	    #  /map 0.324256 -0.192411 0.0242112
	    #   /armbase     <xyz> .463256 -.192411 .021589 </xyz>

	    #  /arm_base_link 0.2, 0.2, -0.040
	    # offset 	-.263256	.392411		.061589	
        #print "Attempting to grasph the first block."
        #grasp_action("/map",  0.4, 0.0,  0.0242112) #this should get the left block

        #rospy.sleep(0.5)

        #print "Attempting to grasph the second block."
        #grasp_action("/arm_base_link", 0.28, 0.0, -0.040) #this should get the right block
    
        #rospy.sleep(0.5)
        
        #this does not work!
        #print "Attempting to reach with the arm."
        #reach_at("/arm_base_link", 0.2, 0.2, -0.040)
        #reach_at("/arm_base_link", 0.2, -0.2, -0.030)
        #reach_at("/arm_base_link", 0.28, 0.0, -0.040)
        #reach_at("/arm_base_link", 0.2, -0.2, -0.02)

        #this works (and the gripper)
        #print "Attempting to move the arm."
        #move_arm(0.0, 0.75, -1.972222, 0.0)
        #rospy.sleep(0.5)
        #move_arm(0.0, 1.972222, -1.972222, 0.0)
        #rospy.sleep(0.5)
	
	#  0.324256 -0.192411 0.0242112

        print "Go to the red block"
        print test_grasp ("/map", 2.324256, -0.192411, 0.0242112)
	
	#Attempt to grasp the red block	
	#test_grasp ("/map", 0.324256, -0.192411, 0.0242112)
        rospy.sleep(0.5)

        print "Go to the green block"
        print test_grasp ("/map",  0.40, 2.0,  0.0242112)

	#test_grasp ("/arm_base_link", 0.2, -0.2, -0.040)

	rospy.sleep(0.5)	

        #test_grasp ("/arm_base_link", 0.28, 0.0, -0.040)

	"""
        print "Laser tilt"
        #tilt_laser(2)
	print "NO TILTED LAZER &\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&."
        print "Look at blocks"
        look_at("/arm_base_link", 0.2825, 0.0, -0.025)
	print "LOOKED_AT &\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&\n&."
        print "Open gripper"
        move_gripper(0.2, -0.2);
        print "Reach left stack"
        reach_at("/arm_base_link", 0.2, 0.2, -0.040)
        rospy.sleep(0.5)
        print "Close gripper"
        move_gripper(-0.075, 0.075)
        print "Raise arm"
        move_arm(0.0, 0.75, -1.972222, 0.0)
        print "Reach right stack"
        reach_at("/arm_base_link", 0.2, -0.2, -0.030)
        rospy.sleep(0.5)
        print "Open gripper"
        move_gripper(0.2, -0.2)
        print "Raise arm"
        move_arm(0.0, 0.75, -1.972222, 0.0)
        print "Reach middle stack"
        reach_at("/arm_base_link", 0.28, 0.0, -0.040)
        rospy.sleep(0.5)
        print "Close gripper"
        move_gripper(-0.075, 0.075)
        print "Raise arm"
        move_arm(0.0, 0.75, -1.972222, 0.0)
        print "Reach right stack"
        reach_at("/arm_base_link", 0.2, -0.205, 0.035)
        rospy.sleep(0.5)
        print "Open gripper"
        move_gripper(0.2, -0.2)
        print "Reset arm"
        move_arm(0.0, 1.972222, -1.972222, 0.0)

        print "Blocks-stacking actions completed."
	"""
    except rospy.ROSInterruptException:
        pass

